#include "pcpt_comm/pcpt_comm_car.h"

/**************************************************************************************
功能描述: 初始化
修改记录:
**************************************************************************************/
INT32 DYNC_CAR_EX_C::Init()
{
    INT32 rslt = objCfgParam_.Init();
    if (rslt != 0) {
        ST_LOG_ERR("objCfgParam_.Init() Fail.");
        return -1;
    }

    COOR_2D_POSE_STRU stScanCoorSysPoseByCar;
    stScanCoorSysPoseByCar.stPos.SetZero();
    stScanCoorSysPoseByCar.dirAngle = 0;
    objCarToScanTf_.SetTransRelationship(stScanCoorSysPoseByCar);

    COOR_2D_POSE_STRU stBackScanCoorSysPoseByCar;
    stBackScanCoorSysPoseByCar.stPos.SetZero();
    stBackScanCoorSysPoseByCar.dirAngle = PI;
    objCarToBackScanTf_.SetTransRelationship(stBackScanCoorSysPoseByCar);
    
    InitDirectedLine();
    
    return 0;
}

/**************************************************************************************
功能描述: 初始化有向线
修改记录:
**************************************************************************************/
void DYNC_CAR_EX_C::InitDirectedLine()
{
    float dirAngle    = AdjustAngle(-0.5 * PI);                         //相对于车体方向的右向线
    float dirAngleSin = sin(dirAngle);
    float dirAngleCos = cos(dirAngle);

    COORDINATE_2D_STRU  stLeftHeadPoint;
    GetLeftHeadPosByCarWithProtect(0, 0, stLeftHeadPoint);
    objHeadRightwardLine_.Reset(stLeftHeadPoint, dirAngleSin, dirAngleCos);
    
    GetBackCameraPoseByCar(stBackCameraPoseByCar_);
    objBackCameraRightwardLine_.Reset(stBackCameraPoseByCar_.stPos, dirAngleSin, dirAngleCos);

    COORDINATE_2D_STRU stCarPos;
    stCarPos.SetZero();
    objBackAxisRightwardLine_.Reset(stCarPos, dirAngleSin, dirAngleCos);

    dirAngle = 0;
    dirAngleSin = sin(dirAngle);
    dirAngleCos = cos(dirAngle);
    objBodyCenterForwardLine_.Reset(stCarPos, dirAngleSin, dirAngleCos);
}

/**************************************************************************************
功能描述: 获取车体后camera位姿
修改记录:
**************************************************************************************/
void DYNC_CAR_EX_C::GetBackCameraPoseByCar(COOR_2D_POSE_STRU &stCameraPoseByCar)
{
    CFG_PARAM_INSTALL_POS_STRU stInstallPosParam;
    stInstallPosParam          = objCfgParam_.GetBackDepthCameraInstallCfg();
    stCameraPoseByCar.stPos.x  = stInstallPosParam.tx;
    stCameraPoseByCar.stPos.y  = stInstallPosParam.ty;
    stCameraPoseByCar.dirAngle = PI;
}

/**************************************************************************************
功能描述: 车体坐标系下，二维点坐标是否在车前右向线的后侧
修改记录:
**************************************************************************************/
BOOL DYNC_CAR_EX_C::IsPointBehindHead(const COORDINATE_2D_STRU &stPoint) const
{
    return objHeadRightwardLine_.IsPointOnRight(stPoint);    
}
/**************************************************************************************
功能描述: 车体坐标系下，二维点坐标是否在后轴右向线的后侧
修改记录:
**************************************************************************************/
BOOL DYNC_CAR_EX_C::IsPointBehindBackAxis(const COORDINATE_2D_STRU &stPoint) const
{
    return objBackAxisRightwardLine_.IsPointOnRight(stPoint);
}

/**************************************************************************************
功能描述: 车体坐标系下，二维点坐标是否在车体中轴线的左侧
修改记录:
**************************************************************************************/
BOOL DYNC_CAR_EX_C::IsPointInLeftSide(const COORDINATE_2D_STRU &stPoint) const
{
    return objBodyCenterForwardLine_.IsPointOnLeft(stPoint);
}

/**************************************************************************************
功能描述: 是否为危险区域
修改记录:
**************************************************************************************/
BOOL DYNC_CAR_EX_C::IsPointInGoBackDangerArea(const COORDINATE_2D_STRU &stObstPosByCar) const
{
    /* 车尾摄像头之后的距离为危险区域 */
    return (objBackCameraRightwardLine_.IsPointOnRight(stObstPosByCar) && (stObstPosByCar.CalcDistSqu(stBackCameraPoseByCar_.stPos) < (5.0 * 5.0)));
}

/**************************************************************************************
功能描述: 
修改记录:
**************************************************************************************/
void DYNC_CAR_EX_C::GetScanPointPosByLaser(unsigned int angleIdx, float dist, COORDINATE_2D_STRU &stPosByLaser) const
{
    objGeneralLaser_.GetPointPosByLaser(angleIdx, dist, &stPosByLaser);
}

/**************************************************************************************
功能描述: 
修改记录:
**************************************************************************************/
void DYNC_CAR_EX_C::GetForwardScanPointPosByCar(unsigned int angleIdx, float dist, COORDINATE_2D_STRU &stPosByCar) const
{
    COORDINATE_2D_STRU  stPosByLaser;
    GetScanPointPosByLaser(angleIdx, dist, stPosByLaser);
    objCarToScanTf_.TransSonPosToFather(stPosByLaser, stPosByCar);
}

/**************************************************************************************
功能描述: 
修改记录:
**************************************************************************************/
void DYNC_CAR_EX_C::GetBackwardScanPointPosByCar(unsigned int angleIdx, float dist, COORDINATE_2D_STRU &stPosByCar) const
{
    COORDINATE_2D_STRU  stPosByLaser;
    GetScanPointPosByLaser(angleIdx, dist, stPosByLaser);
    objCarToBackScanTf_.TransSonPosToFather(stPosByLaser, stPosByCar);
}

/**************************************************************************************
功能描述: 
修改记录:
**************************************************************************************/
void DYNC_CAR_EX_C::GetForwardScanPointPosByMap(unsigned int angleIdx, float dist, COORDINATE_2D_STRU &stPosByMap) const
{
    COORDINATE_2D_STRU  stPosByCar;
    GetForwardScanPointPosByCar(angleIdx, dist, stPosByCar);
    TransformPosByCarToByMap(stPosByCar, stPosByMap);
}

/**************************************************************************************
功能描述: 
修改记录:
**************************************************************************************/
void DYNC_CAR_EX_C::GetBackwardScanPointPosByMap(unsigned int angleIdx, float dist, COORDINATE_2D_STRU &stPosByMap) const
{
    COORDINATE_2D_STRU  stPosByCar;
    GetBackwardScanPointPosByCar(angleIdx, dist, stPosByCar);
    TransformPosByCarToByMap(stPosByCar, stPosByMap);
}